# http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29
#!/usr/bin/env python
# license removed for brevity
import rospy
from arm_control.msg import JointControl
from arm_control.msg import JointInformation

# [arm_control/JointControl]:
# float32[7] joint_pos
# float32[7] joint_vel
# float32[7] joint_cur
# int32 mode
#
# (base) k@k-Lenovo-Legion-R9000P2021H:~/X5/arx-follow-V2.1$ rosmsg show JointInformation
# [arm_control/JointInformation]:
# float32[7] joint_pos
# float32[7] joint_vel
# float32[7] joint_cur
# int32 mode


def callback_ji2(data):
    a = 1
    # print("ji2")
    # rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
def callback_ji1(data):
    a = 1
    # print("ji1")
    # rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
def callback_jc2(data):
    a = 1
    # print("jc2")
    # rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
def callback_jc1(data):
    a = 1
    a = data.joint_pos[0]
    b = data.joint_vel[0]
    c = data.joint_cur[0]
    action
    # rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
class ArxRobotFollow:
    def __init__(self):
        print("init robot")

    def callback_ji(data):
        a = 1
        # print("jc2")
        # rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
    def callback_jc(data):
        a = 1
        a = data.joint_pos[0]
        b = data.joint_vel[0]
        c = data.joint_cur[0]
        action
        # rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)

if __name__ == '__main__':
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("/joint_information", JointInformation, callback_ji2)
    rospy.Subscriber("/joint_information2", JointInformation, callback_ji1)
    rospy.Subscriber("/joint_control", JointControl, callback_jc2)
    rospy.Subscriber("/joint_control", JointControl, callback_jc1)

    # pub = rospy.Publisher('chatter', JointInformation, queue_size=10)
    # pub = rospy.Publisher('chatter', JointControl, queue_size=10)
    rate = rospy.Rate(50) # 10hz
    rospy.spin()


# follow1_pos_back
# /follow2_pos_back
# /joint_control
# /joint_control2
# /joint_information
# /joint_information2
# /joy
# /master1_pos_back
# /master2_pos_back
# /rosout
# /rosout_agg

